/*
 * This is the 1999 rewrite of IP Firewalling, aiming for kernel 2.3.x.
 *
 * Copyright (C) 1999 Paul `Rusty' Russell & Michael J. Neuling
 */
#include <linux/module.h>
#include <linux/sched.h>
#include <linux/interrupt.h>
#include <linux/spinlock.h>
#include <linux/if_ether.h>
#include <linux/netfilter_ipv4/ip_tables.h>
#include <linux/netfilter_ipv4/ipt_state.h>
#include <asm/uaccess.h>
#include <linux/netdevice.h>
#include "ipt_ipmac.h"

#define HASH_SIZE 4097
struct IPMAC_table {
	int rule_num;
	struct hash_table {
		struct IPMAC ipmac;
		struct hash_table *next;
		struct hash_table *mac_next;
	} **iphash_list,**machash_list;	
};

static struct IPMAC_table table;
static rwlock_t lock=RW_LOCK_UNLOCKED;

int
hashkey(__u32 addr)
{
	return addr % HASH_SIZE;
}

unsigned int
mhashkey(char* mac)
{
	unsigned int tmp;
	tmp = mac[0]^mac[2]^mac[4];
	tmp = (tmp << 5) | (mac[1]^mac[3]^mac[5]);
	return tmp % HASH_SIZE;
}
int
addlist(struct IPMAC *ipmac)
{
	unsigned int key, mkey;
	int ret;
	struct hash_table *tmp1, *tmp2, *tmp3, *mtmp1;
	key = hashkey(ipmac->ip.s_addr);
	mkey = mhashkey(ipmac->mac);
	ret = 0;
        if (!(tmp3 = kmalloc(sizeof(struct hash_table), GFP_KERNEL))) 
		return -ENOMEM;
	write_lock_bh(&lock); 
	tmp1 = tmp2 = table.iphash_list[key];
	while (tmp1) {
		if (tmp1->ipmac.ip.s_addr == ipmac->ip.s_addr) {
			ret = -1;
			break;
		}
		tmp2 = tmp1;
		tmp1 = tmp1->next;
	}
	if (!ret) {
		if (tmp2)
			tmp2->next = tmp3;
		else
			table.iphash_list[key]=tmp3;
		tmp3->next = NULL;
		memcpy(&(tmp3->ipmac),ipmac,sizeof(struct IPMAC));
		/* add node to current mac_hash_list */
		mtmp1 = table.machash_list[mkey];
		table.machash_list[mkey] = tmp3;
		tmp3->mac_next = mtmp1;
		table.rule_num++;
	} else
		kfree(tmp3);
	write_unlock_bh(&lock); 
	return ret;
}	

int
dellist(struct IPMAC *ipmac)
{
	int ret;
	unsigned int key,mkey;
	struct hash_table *tmp1, *tmp2,*mtmp1, *mtmp2;
	key = hashkey(ipmac->ip.s_addr);
	mkey = mhashkey(ipmac->mac);
	ret = -1;
	write_lock_bh(&lock); 
	tmp1 = tmp2 = table.iphash_list[key];
	mtmp1 = mtmp2 = table.machash_list[mkey];
	while (tmp1) {
		if (tmp1->ipmac.ip.s_addr == ipmac->ip.s_addr) {
			ret = 0;
			break;
		}
		tmp2 = tmp1;
		tmp1 = tmp1->next;
	}

	/* rm link from mac hash list */
	while (mtmp1 && (!ret)) {
		if (mtmp1->ipmac.ip.s_addr != ipmac->ip.s_addr) {
			mtmp2 = mtmp1;
			mtmp1 = mtmp1->mac_next;
		}
		else{
			if(mtmp1 != table.machash_list[mkey]){
				mtmp2->mac_next = mtmp1->mac_next;
			}
			else table.machash_list[mkey] = mtmp1->mac_next;
		}	
	}

	if (!ret) {
		if (tmp1 != table.iphash_list[key]) {
			tmp2->next = tmp1->next;
			kfree(tmp1);
		} else {
			table.iphash_list[key] = tmp1->next;
			kfree(tmp1);
		}	
		table.rule_num--;
	}
	write_unlock_bh(&lock);
	return ret;
}

void
flushlist(void)
{
	int i;
	struct hash_table *tmp1, *tmp2;
	write_lock_bh(&lock);
	/*flush the mac hash list */
	for (i = 0; i < HASH_SIZE; i++) 
		table.machash_list[i]=NULL; 

	for (i = 0; i < HASH_SIZE; i++) {
		tmp1 = tmp2 = table.iphash_list[i];
		while (tmp1) {
			tmp2 = tmp1->next;
			kfree(tmp1);
			tmp1 = tmp2;
		}
		table.iphash_list[i]=NULL;
	}
	table.rule_num=0;
	write_unlock_bh(&lock);
}	

static int
is_entry_bridge_mac(unsigned char *p_mac)
{
	register struct net_device* dev = NULL;
	//for (dev = dev_base; dev != NULL; dev = dev->next)
	for (dev = first_net_device(&init_net); dev != NULL; dev = next_net_device(dev)) 
	{
#ifdef DEBUG
		printk(KERN_EMERG "ifname: %s, hwaddr: %x:%x:%x:%x:%x:%x\n", 
				   dev->name, dev->dev_addr[0], dev->dev_addr[1], 
				   dev->dev_addr[2], dev->dev_addr[3], dev->dev_addr[4], dev->dev_addr[5]);
#endif
		if (memcmp(p_mac, dev->dev_addr, ETH_ALEN) == 0 && dev->name[0] == 'b' && dev->name[1] == 'r')
			return 1;
	}
	return 0;
}

static int 
match(const struct sk_buff *skb, 
	const struct net_device *in, 
	const struct net_device *out, 
	const struct xt_match *match,
	const void *matchinfo,
      	int offset, 
	unsigned int protoff,
	int *hotdrop)
{
	unsigned int key,mkey;
	int ret=0;
	struct iphdr *ip;
	struct hash_table *tmp, *mtmp;
	read_lock(&lock); 
	ip = ip_hdr(skb);
	key = hashkey(ip->saddr);
	mkey = mhashkey(eth_hdr(skb)->h_source);
	for (tmp = table.iphash_list[key]; tmp != NULL; tmp = tmp->next){
		if (ip->saddr == tmp->ipmac.ip.s_addr) {
#ifdef DEBUG
			printk(KERN_EMERG "-------------------------\n");
			printk(KERN_EMERG "sip in tables: %d.%d.%d.%d,smac in tables: %x:%x:%x:%x:%x:%x\n", tmp->ipmac.ip.s_addr&0x000000ff, (tmp->ipmac.ip.s_addr&0x0000ff00)>>8, (tmp->ipmac.ip.s_addr&0x00ff0000)>>16, (tmp->ipmac.ip.s_addr&0xff000000)>>24, tmp->ipmac.mac[0], tmp->ipmac.mac[1], tmp->ipmac.mac[2], tmp->ipmac.mac[3], tmp->ipmac.mac[4], tmp->ipmac.mac[5]);
			printk(KERN_EMERG "entry sip: %d.%d.%d.%d,entry smac: %x:%x:%x:%x:%x:%x\n", ip->saddr&0x000000ff, (ip->saddr&0x0000ff00)>>8, (ip->saddr&0x00ff0000)>>16, (ip->saddr&0xff000000)>>24, eth_hdr(skb)->h_source[0], eth_hdr(skb)->h_source[1], eth_hdr(skb)->h_source[2], eth_hdr(skb)->h_source[3], eth_hdr(skb)->h_source[4], eth_hdr(skb)->h_source[5]);
#endif
			if(is_entry_bridge_mac(eth_hdr(skb)->h_source)) {
				read_unlock(&lock);
				return 0;
			}
			if(memcmp(tmp->ipmac.mac, eth_hdr(skb)->h_source, ETH_ALEN)==0) {
				read_unlock(&lock);
				return 0;
			}
			read_unlock(&lock);
			return 1;
		}
	}
	/* not returned yet, We compare the mac address */
	for (mtmp = table.machash_list[mkey]; mtmp != NULL; mtmp = mtmp->mac_next){
		if(memcmp(mtmp->ipmac.mac, eth_hdr(skb)->h_source, ETH_ALEN)==0) {
			ret = 1;
			break;
		}
	}
	read_unlock(&lock);
	return ret;
}

int 
check(const char *tablename, 
	const void *ip,
	const struct xt_match *match,
        void *matchinfo,
        unsigned int hook_mask)
{
#ifdef DEBUG
	printk("calling iptables ipmac ");

#endif
	//ggmm if (matchsize != IPT_ALIGN(0))
		return 0;

	//ggmm return 1;
}

static int
ipt_set_ctl(struct sock *sk,int cmd, void *user, unsigned int len)
{
	int ret;
	struct IPMAC ipmac;
	switch (cmd) {
		case IPT_ADD_CTL:
			if (copy_from_user(&ipmac, user, sizeof(ipmac)) != 0) {
				ret = -EFAULT;
				break;
			}	
			ret = addlist(&ipmac); 
#ifdef DEBUG
			printk(KERN_EMERG "ip in tables: %d.%d.%d.%d,mac in tables: %x:%x:%x:%x:%x:%x\n", ipmac.ip.s_addr&0x000000ff, (ipmac.ip.s_addr&0x0000ff00)>>8, (ipmac.ip.s_addr&0x00ff0000)>>16, (ipmac.ip.s_addr&0xff000000)>>24, ipmac.mac[0], ipmac.mac[1], ipmac.mac[2], ipmac.mac[3], ipmac.mac[4], ipmac.mac[5]);
#endif
			break;
		case IPT_DEL_CTL:
			if (copy_from_user(&ipmac, user, sizeof(ipmac)) != 0) {
				ret = -EFAULT;
				break;
			}	
			ret = dellist(&ipmac);
			break;
		case IPT_FLUSH_CTL:
			flushlist();
			ret = 0;
			break;
		default:
			ret = -EINVAL;
	}		
	return ret;
}	

static int
ipt_get_ctl(struct sock *sk,int cmd, void *user, int *len)
{
	int i, num, ret;
	struct hash_table *tmp;
	struct IPMAC *data;
	switch (cmd) {
		case IPT_GET_NUM_CTL:
			if (copy_to_user(user, &table.rule_num, *len) != 0)
				ret = -EFAULT;
			else
				ret = 0;
			break;
		case IPT_GET_RULE_CTL:
			num = ret = 0;
			read_lock(&lock);
			if (*len < sizeof(struct IPMAC) * table.rule_num) {
				read_unlock(&lock); 
				ret = -EFAULT;
				break;
			}	
			*len = sizeof(struct IPMAC) * table.rule_num;
			data = (struct IPMAC *) user;
			for (i = 0; i < HASH_SIZE; i++) {
				if (!table.iphash_list[i])
					continue;
				for (tmp = table.iphash_list[i]; tmp; tmp = tmp->next,num++)
					if (copy_to_user(&data[num], &tmp->ipmac, sizeof(struct IPMAC)) != 0) {
						ret = -EFAULT;
						break;
					}
			}	
			read_unlock(&lock);
			break;
		default:
			ret = -EINVAL;
	}
	return ret;
}

/* In linux-2.6.22 kernel using xt_match*/
static struct xt_match IPMAC_match = {
        .name           = "IPMAC",
        .family         = AF_INET,
        .match          = match,
        .matchsize      = IPT_ALIGN(0), //sizeof(struct ipt_tos_info),
        .me             = THIS_MODULE
};

static struct nf_sockopt_ops ipt_sockopts = {
        .pf             = PF_INET,
        .set_optmin     = IPT_EXTRA_CTL,
        .set_optmax     = IPT_EXTRA_CTL+3,
        .set            = ipt_set_ctl,
        .get_optmin     = IPT_EXTRA_CTL,
        .get_optmax     = IPT_EXTRA_CTL+2,
        .get            = ipt_get_ctl,
};	    
static int __init init(void)
{
	int i, ret;

	table.iphash_list = kmalloc(sizeof(struct hash_table *) * HASH_SIZE, GFP_KERNEL);
	table.machash_list = kmalloc(sizeof(struct hash_table *) * HASH_SIZE, GFP_KERNEL);
	if ((!table.iphash_list) || !(table.machash_list))
		return -ENOMEM;
	for (i = 0; i < HASH_SIZE; i++){
		table.iphash_list[i] = NULL;
		table.machash_list[i] = NULL;
	}

	ret = nf_register_sockopt(&ipt_sockopts);
	if (ret < 0) {
		kfree(table.iphash_list);
		kfree(table.machash_list);
		return ret;
	}	
        ret = xt_register_match(&IPMAC_match);
        if (ret < 0) {
        	nf_unregister_sockopt(&ipt_sockopts);
        	kfree(table.iphash_list);
        	kfree(table.machash_list);
        }
        return ret;
}

static void __exit fini(void)
{
        xt_unregister_match(&IPMAC_match); 
       	nf_unregister_sockopt(&ipt_sockopts);
	flushlist();
	kfree(table.iphash_list);
	kfree(table.machash_list);
}

module_init(init);
module_exit(fini);
